/*  -------------------------- (C) COPYRIGHT 2022 Fortiortech ShenZhen ---------------------------*/
/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      xxx.c
 * @author    Fortiortech  Appliction Team
 * @since     Create:2021-07-14
 * @date      Last modify:2022-08-04
 * @note      Last modify author is Leo.li
 * @brief
 */

/*  ----------------------------------------------------------------------------------------------*/
#include <FU68xx_2.h>
#include <Myproject.h>

/* Private variables ---------------------------------------------------------*/
FaultStateType xdata mcFaultSource;
FaultVarible xdata mcFaultDect;
CurrentVarible xdata mcCurVarible;
ProtectVarible xdata mcProtectTime;

/**
 * @brief     保护函数汇总处理
 * @date      2022-07-14
 */
void Fault_Detection(void)
{
#if (CurrentRecoverEnable) // 过流保护恢复使能
    {
        Fault_OverCurrentRecover();
    }
#endif
#if (OverSoftCurrentProtectEnable) // 软件过流保护使能
    {
        Fault_Overcurrent();
    }
#endif
#if (VoltageProtectEnable) // 过压保护使能
    {
        Fault_OverUnderVoltage();
    }
#endif
#if (OverPowerProtectEnable) // 功率保护使能
    {
        Fault_Power();
    }
#endif
#if (StartProtectEnable) // 启动保护使能
    {
        Fault_Start();
    }
#endif
#if (StallProtectEnable) // 堵转保护使能
    {
        Fault_Stall();
    }
#endif
#if (PhaseLossProtectEnable) // 缺相保护使能
    {
        Fault_phaseloss();
    }
#endif
}

/**
 * @brief     保护处理函数，关闭FOC输出
 * @date      2022-07-14
 */
void FaultProcess(void)
{
    MOE = 0;
    ClrBit(DRV_CR, FOCEN); // 关闭FOC
}

/**
 * @brief     过压欠压保护函数：程序每5ms判断一次，母线电压大于过压保护值时，计数器加一，计数器值超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
              同理，欠压保护。
              电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，过压恢复值以下时，计数器加一，超过200次后，恢复。
 * @date      2022-07-14
 */
void Fault_OverUnderVoltage(void)
{
    // 过压保护
    if (mcFaultSource == FaultNoSource) // 程序无其他保护下
    {
        if (mcFocCtrl.mcDcbusFlt > OVER_PROTECT_VALUE) // 母线电压大于过压保护值时，计数，超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
        {
            mcFaultDect.OverVoltDetecCnt++;

            if (mcFaultDect.OverVoltDetecCnt > 80) // 检测100ms
            {
                mcFaultDect.OverVoltDetecCnt = 0;
                mcFaultSource = FaultOverVoltage;
            }
        }
        else
        {
            if (mcFaultDect.OverVoltDetecCnt > 0)
            {
                mcFaultDect.OverVoltDetecCnt--;
            }
        }

        // 欠压保护
        if ((mcFocCtrl.mcDcbusFlt < UNDER_PROTECT_VALUE))
        {
            mcFaultDect.UnderVoltDetecCnt++;

            if (mcFaultDect.UnderVoltDetecCnt > 60) // 检测100ms
            {
                MOE = 0;
                ClrBit(DRV_CR, FOCEN);
                mcFaultDect.UnderVoltDetecCnt = 0;
                mcFaultSource = FaultUnderVoltage;
                mcFaultDect.VoltDetecBraketCount = 500;
                mcFaultDect.VoltDetecBraketDuty = 0;
                mcFaultDect.FlagBrakeInit = 1;
            }
        }
        //        if (mcFocCtrl.ZeroLossFlag==1)
        //        {
        //            mcFaultDect.UnderVoltDetecCnt++;
        //
        //            if (mcFaultDect.UnderVoltDetecCnt > 0) //检测100ms
        //            {
        //                MOE = 0;
        //                ClrBit(DRV_CR, FOCEN);
        //                mcFaultDect.UnderVoltDetecCnt = 0;
        //                mcFaultSource = FaultUnderVoltage;
        //                mcFaultDect.VoltDetecBraketCount = 500;
        //                mcFaultDect.VoltDetecBraketDuty  = 0;
        //                mcFaultDect.FlagBrakeInit = 1;
        //
        //            }
        //        }
        else
        {
            if (mcFaultDect.UnderVoltDetecCnt > 0)
            {
                mcFaultDect.UnderVoltDetecCnt--;
            }
        }
    }

#if (VoltageRecoverEnable)
    {
        /*******过压欠压保护恢复*********/
        if ((mcState == mcFault) && ((mcFaultSource == FaultUnderVoltage) || (mcFaultSource == FaultOverVoltage)))
        {
            //            if ((mcFocCtrl.mcDcbusFlt < OVER_RECOVER_VALUE) && (mcFocCtrl.mcDcbusFlt > UNDER_RECOVER_VALUE)&&(mcFocCtrl.ZeroLossFlag==0) )
            if ((mcFocCtrl.mcDcbusFlt < OVER_RECOVER_VALUE) && (mcFocCtrl.mcDcbusFlt > UNDER_RECOVER_VALUE))
            {
                if (mcFaultDect.FlagBrakeInit == 0)
                {
                    mcFaultDect.VoltRecoverCnt++;
                }
                else
                {
                    mcFaultDect.VoltRecoverCnt = 0;
                }

                //                if ((mcFocCtrl.ZeroLossFlag==0)&&(mcFaultDect.VoltRecoverCnt > VoltageRecoverTime1)&&(mcFaultSource == FaultUnderVoltage)) //连续检测1s，若正常则恢复
                if (((mcFaultDect.VoltRecoverCnt > VoltageRecoverTime) && (mcFaultSource == FaultUnderVoltage))     // 欠压恢复，通过时间判断恢复
                  //|| ((mcFocCtrl.ZeroLossFlag == 0) && (mcFaultDect.VoltRecoverCnt > VoltageRecoverTime1) && (mcFaultSource == FaultUnderVoltage)) // 欠压恢复，通过时间判断以及过零丢失恢复
                    || ((mcFaultDect.VoltRecoverCnt > VoltageRecoverTime2) && (mcFaultSource == FaultOverVoltage))) // 过压恢复，通过时间判断恢复
                {
                    mcFaultSource = FaultNoSource;
                    mcFaultDect.VoltRecoverCnt = 0;
                }
            }
            else
            {
                mcFaultDect.VoltRecoverCnt = 0;
            }

            if (mcFaultSource == FaultUnderVoltage)
            {
                if (mcFaultDect.VoltDetecBraketCount > 0)
                {
                    mcFaultDect.VoltDetecBraketCount--;
                }

                if (mcFaultDect.VoltDetecBraketDuty < DRV_ARR)
                {
                    mcFaultDect.VoltDetecBraketDuty += 2;
                }
            }
        }
    }
#endif
}

/**
 * @brief     欠压处理
 * @brief     刹车处理
 * @brief     Flash保存
 * @date      2022-07-14
 */
void UnderProcess(void)
{

    if ((mcFaultSource == FaultOverVoltage) || (mcFaultSource == FaultUnderVoltage))
    {
        /*****过欠压停机刹车功能*****/
        if (((mcFaultDect.VoltDetecBraketCount < 1500) && (mcFaultDect.VoltDetecBraketCount > 1200)) || ((mcFaultDect.VoltDetecBraketCount < 1000) && (mcFaultDect.VoltDetecBraketCount > 700)) || ((mcFaultDect.VoltDetecBraketCount < 500) && (mcFaultDect.VoltDetecBraketCount > 300)) || ((mcFaultDect.VoltDetecBraketCount < 200) && (mcFaultDect.VoltDetecBraketCount > 100)))
        {
            DRV_DR = DRV_ARR; // DRV_DR = mcFaultDect.VoltDetecBraketDuty; // 下桥臂10% duty
            MOE = 1;
            if (mcFaultDect.FlagBrakeInit==1)
            {
                /*关闭输出*/
                DRV_CMR &= 0xFFC0;
                mcFaultDect.FlagBrakeInit = 0;
                ClrBit(DRV_CR, OCS);   // OCS = 0, PWM来源DRV_COMR
                DRV_CMR |= 0x15;       // UVW相下桥输出
                SetBit(DRV_CR, DRVEN); /*DRV计数器使能，0-禁止，1-使能*/
                MOE = 1;
                DRV_DR = 0.03 * DRV_ARR;
            }
        }
        else if (mcFaultDect.VoltDetecBraketCount <= 0)
        {
            MOE = 0;
            DRV_OUT = 0x00;
        }
        else
        {
            MOE = 0;
        }

        if (mcFaultDect.VoltDetecBraketCount <= 50)
        {
            RGB(0, 0, 0);
        }
    }
}
/**
 * @brief     功率保护函数
 * @date      2022-07-14
 */
void Fault_Power(void)
{
    if (mcFaultSource == FaultNoSource) // 程序无其他保护下
    {
        if (mcFocCtrl.Powerlpf > PowerLimit) // 功率大于保护值时计数，超过20次，判断为过载保护，关闭输出;反之，计数器慢慢减
        {
            mcFaultDect.OverPowerDetecCnt++;

            if (mcFaultDect.OverPowerDetecCnt > 180)
            {
                mcFaultDect.OverPowerDetecCnt = 0;
                mcFaultSource = FaultOverPower;
            }
        }
        else
        {
            if (mcFaultDect.OverPowerDetecCnt > 0)
            {
                mcFaultDect.OverPowerDetecCnt--;
            }
        }
    }

#if ((SPEED_MODE != ONOFFTEST) && (PowerRecoverEnable)) // 功率保护恢复使能
    {
        Fault_OverPowerRecover();
    }
#endif
}

/**
 * @brief     功率保护恢复函数
 * @date      2022-07-14
 */
void Fault_OverPowerRecover(void)
{
    if ((mcState == mcFault) && (mcFaultSource == FaultOverPower) && (mcProtectTime.PowerPretectTimes < PowerRecoverTimes))
    {
        mcFaultDect.OverPowerDetecCnt++;

        if (mcFaultDect.OverPowerDetecCnt >= OverPowerRecoverTime)
        {
            mcFaultDect.OverPowerDetecCnt = 0;
            mcProtectTime.PowerPretectTimes++;
            mcFaultSource = FaultNoSource;
        }
    }
}

/**
 * @brief     电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则OverCurCnt加1
 * @date      2022-07-14
 */
void Fault_Overcurrent(void)
{
    if ((mcState == mcRun) || (mcState == mcStart)) // check over current in rum and open mode
    {
        mcCurVarible.Is = SqrtI_alpbet(FOC__IA, FOC__IBET);

        if (mcCurVarible.Is >= OverSoftCurrentValue) // 过温保护
        {
            if (mcCurVarible.OverCurCnt < OverSoftCurrent_DectTime)
            {
                mcCurVarible.OverCurCnt++;
            }
            else
            {
                mcCurVarible.OverCurCnt = 0;
                mcFaultSource = FaultSoftOVCurrent;
            }
        }
        else
        {
            mcCurVarible.OverCurCnt = 0;
        }
    }
}

/**
 * @brief     软硬件过流保护恢复
 * @date      2022-07-14
 */
void Fault_OverCurrentRecover(void)
{
    if ((mcState == mcFault) && ((mcFaultSource == FaultSoftOVCurrent) || (mcFaultSource == FaultHardOVCurrent)) && (mcProtectTime.CurrentPretectTimes < 5))
    {
        mcFaultDect.CurrentRecoverCnt++;

        if (mcFaultDect.CurrentRecoverCnt >= OverCurrentRecoverTime) // 1000*5=5s
        {
            mcFaultDect.CurrentRecoverCnt = 0;
            mcProtectTime.CurrentPretectTimes++;
            mcFaultSource = FaultNoSource;
        }
    }
}

/**
 * @brief     启动检测
 * @date      2022-07-14
 */
void Fault_Start(void)
{
    /*******启动保护恢复*********/
    if (mcState == mcRun)
    {
        // 方法一，5s内速度大于最大速度，同时反电动势值低于一定值
        if (mcFaultDect.StartSpeedCnt <= 1000)
        {
            mcFaultDect.StartSpeedCnt++;
            if (mcFaultDect.StartSpeedCnt > 300)
            {
                if ((mcFocCtrl.SpeedFlt > Motor_Max_Speed) && (mcFocCtrl.EsValue < 5000))
                {
                    mcFaultDect.StartSpeedTime++;
                    if (mcFaultDect.StartSpeedTime > 100)
                    {
                        mcFaultDect.StartSpeedTime = 0;
                        mcFaultDect.StartSpeedCnt = 0;
                        mcFaultSource = FaultStart;
                        mcProtectTime.SecondStartTimes++;
                        mcProtectTime.StartFlag = 1;
                    }
                }
            }
        }

        // 方法二
        if (mcFaultDect.StartEsCnt <= 1000) // 前1s，等待1.5s后，开始判断Es，如果超过一定次数，则失败
        {
            mcFaultDect.StartEsCnt++;
            mcFaultDect.StartDelay++;

            if (mcFaultDect.StartDelay >= 300) // 1.5s
            {
                mcFaultDect.StartDelay = 300;

                if ((mcFocCtrl.EsValue < 1000) && (mcFocCtrl.CtrlMode == 0))
                {
                    mcFaultDect.StartESCount++;

                    if (mcFaultDect.StartESCount >= 90) // 50次Es小于设定值，启动失败
                    {
                        mcFaultSource = FaultStart;
                        mcProtectTime.SecondStartTimes++;
                        mcFaultDect.StartDelay = 0;
                        mcFaultDect.StartESCount = 0;
                        mcProtectTime.StartFlag = 2;
                    }
                }
                else
                {
                    if (mcFaultDect.StartESCount > 0)
                    {
                        mcFaultDect.StartESCount--;
                    }
                }
            }
        }
        else
        {
            mcFaultDect.StartESCount = 0;
        }

        // 方法三，长时间在CtrlMode=0状态
        if (mcFocCtrl.CtrlMode == 0)
        {
            mcFaultDect.StartFocmode++;

            if (mcFaultDect.StartFocmode >= 2000) // 在MODE0模式超过2s起动失败
            {
                mcFaultDect.StartFocmode = 0;
                mcFaultSource = FaultStart;
                mcProtectTime.SecondStartTimes++;
                mcProtectTime.StartFlag = 3;
            }
        }
    }

// 起动保护恢复
#if ((SPEED_MODE != ONOFFTEST) && (StartRecoverEnable)) // 起停测试时屏蔽起动保护恢复
    {
        if ((mcFaultSource == FaultStart) && (mcState == mcFault) && (mcProtectTime.SecondStartTimes <= StartRecoverTimes))
        {
            mcFaultDect.StartRecoverDelayCnt++;

            if (mcFaultDect.StartRecoverDelayCnt > StartRecoverTime) // 启动保护延时恢复时间
            {
                mcFaultDect.StartRecoverDelayCnt = 0;
                mcFaultSource = FaultNoSource;
            }
        }
    }
#endif
}

/**
 * @brief     堵转检测
 * @date      2022-07-14
 */
void Fault_Stall(void)
{
    if (mcState == mcRun)
    {
        if (mcFaultDect.StallDelayCnt <= 1000) // 启动后延时5s判断是否堵转保护
        {
            mcFaultDect.StallDelayCnt++;
        }
        else
        {
            // method 1，当反电动势太小 或当 转速太大但反电动势却很小时进入保护
            // if ((mcFocCtrl.EsValue < 3200) || ((FOC__EOME > _Q15(10000.0 / MOTOR_SPEED_BASE)) && (mcFocCtrl.EsValue < 3200)))
            if ((mcFocCtrl.EsValue < 3200) || ((FOC__EOME > _Q15(10000.0 / MOTOR_SPEED_BASE)) && (mcFocCtrl.EsValue < 3200))) // 精油8000转mcFocCtrl.EsValue=2362
            {
                mcFaultDect.StallDectEs++;

                if (mcFaultDect.StallDectEs >= 80) // 判断满足条件达到设置次数，保护动作
                {
                    mcFaultDect.StallDectEs = 0;
                    mcFaultSource = FaultStall;
                    mcProtectTime.StallTimes++;    // 堵转次数+1
                    mcFaultDect.StallDelayCnt = 0; // 保护后重启开始重新计数，保持5S延迟
                    mcProtectTime.StallFlag = 1;
                }
            }
            else
            {
                if (mcFaultDect.StallDectEs > 0)
                {
                    mcFaultDect.StallDectEs--;
                }
            }

            // method 2，判断速度低于堵转最小值或者超过堵转最大值
            if ((mcFocCtrl.SpeedFlt < Motor_Stall_Min_Speed) || (mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
            {
                mcFaultDect.StallDectSpeed++;

                if (mcFaultDect.StallDectSpeed >= 70) // 判断满足条件达到设置次数，保护动作
                {
                    mcFaultDect.StallDectSpeed = 0;
                    mcFaultSource = FaultStall;
                    mcProtectTime.StallTimes++;    // 堵转次数+1
                    mcFaultDect.StallDelayCnt = 0; // 保护后重启开始重新计数，保持5S延迟
                    mcProtectTime.StallFlag = 2;
                }
            }
            else
            {
                if (mcFaultDect.StallDectSpeed > 0)
                {
                    mcFaultDect.StallDectSpeed--;
                }
            }
        }
#if (Stallmethod3ProtectEnable)
        {
            // method 3，类转动惯量判断堵转
            if (mcFaultDect.StallDelayCnt1 <= 400) // 启动后延时5s判断是否堵转保护
            {
                mcFaultDect.StallDelayCnt1++;
            }
            else
            {
                mcFaultDect.IqRef = FOC_IQREF;
                mcFaultDect.SpeedAcc = mcFocCtrl.SpeedFlt - mcFocCtrl.SpeedFlt_Pre; // 速度差
                mcFocCtrl.SpeedFlt_Pre = mcFocCtrl.SpeedFlt;
                if ((mcFaultDect.SpeedAcc <= 0) /*|| ((SpeedPICtrl.ExtOut < I_Value(0.2)))*/) // 减速和电流小不判断，电流小判断有误
                {
                    mcFaultDect.J_inverse = 0;
                }
                else if (mcFaultDect.SpeedAcc >= mcFaultDect.IqRef) // 数据溢出
                {
                    mcFaultDect.J_inverse = 65535;
                }
                else // 加速且电流足够大
                {
                    mcFaultDect.J_inverse = DivQ_L_MDU(mcFaultDect.SpeedAcc, 0, mcFaultDect.IqRef);
                }
                mcFaultDect.J_inverse_lpf = LPFFunction(mcFaultDect.J_inverse, mcFaultDect.J_inverse_lpf, 10); // 注意低通滤波器系数范围为0---127

                if (mcFaultDect.J_inverse_lpf > 5000)
                {
                    mcFaultDect.JDectCnt++;
                    if (mcFaultDect.JDectCnt >= 80)
                    {
                        mcFaultDect.JDectCnt = 0;
                        mcFaultSource = FaultStall;
                        mcProtectTime.StallFlag = 3;
                        mcProtectTime.StallTimes++; // 堵转次数+1
                    }
                }
                else
                {
                    if (mcFaultDect.JDectCnt > 0)
                    {
                        mcFaultDect.JDectCnt--;
                    }
                }
            }
        }
#endif
    }

#if ((SPEED_MODE != ONOFFTEST) && (StallRecoverEnable))
    {
        /*******堵转保护恢复*********/
        if ((mcFaultSource == FaultStall) && (mcState == mcFault) && (mcProtectTime.StallTimes <= StallRecoverTimes)) // 堵转重启次数
        {
            mcFaultDect.StallReCount++;

            if (mcFaultDect.StallReCount >= StallRecoverTime)
            {
                mcFaultDect.StallReCount = 0;
                mcFaultSource = FaultNoSource;
            }
        }
        else
        {
            mcFaultDect.StallReCount = 0;
        }
    }
#endif
}

/**
 * @brief     缺相保护检测
 * @date      2022-07-14
 */
void Fault_phaseloss(void)
{
    if (mcState == mcRun)
    {
        if (mcFaultDect.Lphasecnt < 500)
        {
            mcFaultDect.Lphasecnt++;
        }
        else
        {
            mcCurVarible.Max_ia = FOC__IAMAX;
            mcCurVarible.Max_ib = FOC__IBMAX;
            mcCurVarible.Max_ic = FOC__ICMAX;

            if (((mcCurVarible.Max_ia > (mcCurVarible.Max_ib * 3)) || (mcCurVarible.Max_ia > (mcCurVarible.Max_ic * 3))) && (mcCurVarible.Max_ia > PhaseLossCurrentValue))
            {
                mcFaultDect.AOpencnt++;
            }
            else
            {
                if (mcFaultDect.AOpencnt > 0)
                {
                    mcFaultDect.AOpencnt--;
                }
            }

            if (((mcCurVarible.Max_ib > (mcCurVarible.Max_ia * 3)) || (mcCurVarible.Max_ib > (mcCurVarible.Max_ic * 3))) && (mcCurVarible.Max_ib > PhaseLossCurrentValue))
            {
                mcFaultDect.BOpencnt++;
            }
            else
            {
                if (mcFaultDect.BOpencnt > 0)
                {
                    mcFaultDect.BOpencnt--;
                }
            }

            if (((mcCurVarible.Max_ic > (mcCurVarible.Max_ia * 3)) || (mcCurVarible.Max_ic > (mcCurVarible.Max_ib * 3))) && (mcCurVarible.Max_ic > PhaseLossCurrentValue))
            {
                mcFaultDect.COpencnt++;
            }
            else
            {
                if (mcFaultDect.COpencnt > 0)
                {
                    mcFaultDect.COpencnt--;
                }
            }

            mcCurVarible.Max_ia = 0;
            mcCurVarible.Max_ib = 0;
            mcCurVarible.Max_ic = 0;
            SetBit(FOC_CR3, ICLR);

            if (mcFaultDect.AOpencnt > PhaseLoss_DectTIME || mcFaultDect.BOpencnt > PhaseLoss_DectTIME || mcFaultDect.COpencnt > PhaseLoss_DectTIME)
            {
                mcProtectTime.LossPHTimes++;
                mcFaultSource = FaultLossPhase;
            }
        }
    }

/*******缺相保护恢复*********/
#if ((SPEED_MODE != ONOFFTEST) && (PhaseLossRecoverEnable)) // 起停测试时屏蔽缺相保护恢复
    {
        if ((mcFaultSource == FaultLossPhase) && (mcState == mcFault) && (mcProtectTime.LossPHTimes < PhaseLossRecoverTimes)) // 可重启5次
        {
            mcFaultDect.mcLossPHRecCount++;

            if (mcFaultDect.mcLossPHRecCount >= PhaseLossRecoverTime)
            {
                mcFaultDect.AOpencnt = 0;
                mcFaultDect.BOpencnt = 0;
                mcFaultDect.COpencnt = 0;
                mcFaultSource = FaultNoSource;
            }
        }
        else
        {
            mcFaultDect.mcLossPHRecCount = 0;
        }
    }
#endif
}

///*---------------------------------------------------------------------------*/
///*  Name     :   void Fault_RecoverDetection(void)
//    /* Input    :   NO
//    /* Output   :   NO
//    /* Description: 保护恢复函数。
//    /*---------------------------------------------------------------------------*/
// void Fault_RecoverDetection(void)
//{
//
//}